/*
  Copyright (c) 2014 Dimitrij Gester

  "Smart RC TX" is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  "Smart RC TX" is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with "Smart RC TX"; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/


#ifndef SAMPLER_H
#define SAMPLER_H

#include <Arduino.h>
#include "Channels.h"

template<byte capacity = 6>
class Sampler
{
  public:
    Sampler(Channels<capacity> &ch)
    : currentSlot(0)
    , channels(ch)
    {
      // change default ADC prescaler from 128 to 64 to accommodate to the 
      // 'reduced' 8 MHz clock and reach 125 kHz ADC rate.
      // See comment in 'wiring.c:306'
      ADCSRA &= ~_BV(ADPS0);
      ADCSRA |= _BV(ADPS2) | _BV(ADPS1);
    }
    
    void sample()
    {
      for(byte i = 0; i < capacity; ++i)
      {
        this->analogBuffer[i][currentSlot] = analogRead(i);
        this->channels.setChannel(i, analogBuffer[i][currentSlot] - 0x200);
//        this->channels.setChannel(i, median(analogBuffer[i]) - 0x200);
//        this->channels.setChannel(i, analogRead(i));
      }
      
      if(++(this->currentSlot) >= bufferSize)
        this->currentSlot = 0;
    }
    
  private:
    uint16_t median(uint16_t * slots)
    {
      uint16_t mid, high;
      
      high = max(slots[0], slots[1]);
      mid = min(high, slots[2]);
    	
      return mid;
    }
    
  private:
    static const byte bufferSize = 3;
    byte currentSlot;
    uint16_t analogBuffer[capacity][bufferSize];
    Channels<capacity> &channels;
};

#endif

